/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.archwood.frc2607;


import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.SpeedController;
//import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.DigitalInput;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class HomecomingRobot extends IterativeRobot {

    private SpeedController leftFront, rightFront, leftRear, rightRear;
    private Compressor theCompressor;
    private RobotDrive driveTrain;
    private Joystick driveStick;
    private Solenoid fireCannon;
//    private Relay fireCannon;
    private DigitalInput safetySwitch;

    private boolean okToFire, switchInitPos, switchToggled, switchCurPos;

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {

        leftFront = new Jaguar(1);              // leftFront = PWM 1
        rightFront = new Jaguar(2);             // rightFront = PWM 2
        leftRear = new Jaguar(3);               // leftRear = PWM 3
        rightRear = new Jaguar(4);              // rightRear = PWM 4

        driveStick = new Joystick(1);

        safetySwitch = new DigitalInput(14);    // toggle switch = DIO 14 (last)

        fireCannon = new Solenoid(1);           // solenoid output 1
//        fireCannon = new Relay(8);
//        fireCannon.setDirection(Relay.Direction.kForward);

        theCompressor = new Compressor(1, 1);  // pressure switch = DIO 1 (first)
                                               // spike = Relay Output 1

        driveTrain = new RobotDrive(leftFront, leftRear, rightFront, rightRear);
        driveTrain.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
        driveTrain.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {

    }

    public void teleopInit() {
        theCompressor.start();
        okToFire = false;
        switchInitPos = safetySwitch.get();
        switchToggled = false;
    }
    
    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        theCompressor.start();
        driveTrain.arcadeDrive(driveStick.getY(), driveStick.getX());

        switchCurPos = safetySwitch.get();

        if (!okToFire) {
            if (switchToggled && (switchCurPos == switchInitPos)) {
                okToFire = true;
                switchToggled = false;
            }
            if (!switchToggled && (switchCurPos == !switchInitPos))
                switchToggled = true;
        }

        boolean fireButton = driveStick.getRawButton(1);
        if (fireButton)
            if (okToFire) {
                fireCannon.set(true);
                okToFire = false;
            }

        if (!fireButton)
            fireCannon.set(false);

/*
        if (driveStick.getRawButton(1))
            fireCannon.set(Relay.Value.kOn);
        else
            fireCannon.set(Relay.Value.kOff);
*/
 }
    public void disabledInit() {
        theCompressor.stop();        
    }


    public void disabledPeriodic() {

    }
}
